#include "myDevice.h"
#include <string.h>
#define PWM_MAX    4096 //PWM最大限幅
union ByteArrayFloat agv_target_Vy;
union ByteArrayFloat agv_target_Vx;
union ByteArrayFloat agv_limit_lineSpeed={2};//运动最高速度
union ByteArrayFloat agv_limit_rateSpeed={0.3};//旋转最高速度
int16_t compare;

float Car_HW=1; //小车旋转参数比例值
 
float TargetA; // A轮目标速度
float TargetB; // B轮目标速度
float TargetC; // C轮目标速度
float TargetD; // D轮目标速度
double PWM_A,PWM_B,PWM_C,PWM_D;
u16 Va,Vb,Vc,Vd;
u8 input_state[2]={0};
u8 i1,i2,i3,i4,i5,i6,i7,i8,i9,i10,i11,i12;
u8 i13,i14,i15,i16,i17,i18,i19,i20,i21,i22,i23,i24;
void input_detect(void)
{
	i13=P43;
	i14=P27;
	i15=P26;
	i16=P25;
	i17=P24;
	i18=P23;
	i19=P22;
	i20=P21;
	i21=P36;
	i22=P37;
	i23=P41;
	i24=P20;

	input_state[0]=i9<<7|i10<<6|i11<<5|i12<<4|i13<<3|i14<<2|i15<<1|i16;
	input_state[1]=i17<<7|i18<<6|i19<<5|i20<<4|i21<<3|i22<<2|i23<<1|i24;
	// input_state[0]=0xff&i1<<0|input_state[0];
	// input_state[0]=0xff&i2<<1|input_state[0];
	// input_state[0]=0xff&i3<<2|input_state[0];
	// input_state[0]=0xff&i4<<3|input_state[0];
	// input_state[0]=0xff&i5<<4|input_state[0];
	// input_state[0]=0xff&i6<<5|input_state[0];
	// input_state[0]=0xff&i7<<6|input_state[0];
	// input_state[0]=0xff&i8<<7|input_state[0];
}
u8 buf_test[3]={0x3B,0x3B,0xcc};
void input_feedback(void)
{
//	sendToServer(input_state);
	uartData_pack(huart3,0x4003,input_state,2);	
}
void speed_feedback(void)
{
	Va=cnt_int0;
	Vb=cnt_int1;
	Vc=cnt_int2;
	Vd=cnt_int3;
//	printf("A轮速度为%d,B轮速度为%d,C轮速度为%d,D轮速度为%d;",Va,Vb,Vc,Vd);			
	cnt_int0=0;
	cnt_int1=0;
	cnt_int2=0;
	cnt_int3=0;
}
void speed_print(void)
{
	printf("A轮速度为%d;",Va);				
}

void Motor1_Speed(int16_t Compare)//设置速度
{
	if(Compare>=0)//前进
	{
		PWMLevelSet(PWM15_1,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_0,Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_0,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		
	}
	else//后退
	{
		PWMLevelSet(PWM15_0,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_1,-Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_1,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	}
}
void Motor1_Stop(void)//自然停止
{
	PWMLevelSet(PWM15_0,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_1,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor1_Brake(void)//急刹
{
	PWMLevelSet(PWM15_0,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_1,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor2_Speed(int16_t Compare)//设置速度
{
	if(Compare>=0)//前进
	{
		PWMLevelSet(PWM15_2,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_3,Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_3,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		
	}
	else//后退
	{
		PWMLevelSet(PWM15_3,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_2,-Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_2,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	}
}
void Motor2_Stop(void)//自然停止
{
	PWMLevelSet(PWM15_2,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_3,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor2_Brake(void)//急刹
{
	PWMLevelSet(PWM15_2,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_3,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor3_Speed(int16_t Compare)//设置速度
{
	if(Compare>=0)//前进
	{
		PWMLevelSet(PWM15_5,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_4,Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_4,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		
	}
	else//后退
	{
		PWMLevelSet(PWM15_4,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_5,-Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_5,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	}
}
void Motor3_Stop(void)//自然停止
{
	PWMLevelSet(PWM15_4,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_5,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor3_Brake(void)//急刹
{
	PWMLevelSet(PWM15_4,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_5,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor4_Speed(int16_t Compare)//设置速度
{
	if(Compare>=0)//前进
	{
		PWMLevelSet(PWM15_6,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_7,Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_7,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		
	}
	else//后退
	{
		PWMLevelSet(PWM15_7,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
		PWM15Duty(PWM15_6,-Compare,0x000);		//PWM_ID, 输出低电平位置, 输出高电平位置
		PWMLevelSet(PWM15_6,DISABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	}
}
void Motor4_Stop(void)//自然停止
{
	PWMLevelSet(PWM15_6,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_7,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
void Motor4_Brake(void)//急刹
{
	PWMLevelSet(PWM15_6,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_7,DISABLE,ENABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}


void Move_Transfrom(float Vx,float Vy,float Vz)//整车移动量转换为单轮速度  x:前+后-  y:左+右-  z:逆+顺-
{
	TargetA=(Vx+Vy-Vz*Car_HW)*2048;//满速为2m/s,满占空比为0x1000=4096
	TargetB=(Vx-Vy-Vz*Car_HW)*2048;
	TargetC=(Vx+Vy+Vz*Car_HW)*2048;
	TargetD=(Vx-Vy+Vz*Car_HW)*2048;

	PWM_A=TargetA;//PID
	PWM_B=TargetB;
	PWM_C=TargetC;
	PWM_D=TargetD;
	
	//驱动
	if(PWM_A>PWM_MAX){Motor1_Speed(PWM_MAX);}//限幅
	else if(PWM_A<-PWM_MAX){Motor1_Speed(-PWM_MAX);}//限幅
	else{Motor1_Speed((int16_t)PWM_A);}//正常输出
	
	if(PWM_B>PWM_MAX){Motor2_Speed(PWM_MAX);}
	else if(PWM_B<-PWM_MAX){Motor2_Speed(-PWM_MAX);}
	else{Motor2_Speed((int16_t)PWM_B);}
	
	if(PWM_C>PWM_MAX){Motor3_Speed(PWM_MAX);}
	else if(PWM_C<-PWM_MAX){Motor3_Speed(-PWM_MAX);}
	else{Motor3_Speed((int16_t)PWM_C);}
	
	if(PWM_D>PWM_MAX){Motor4_Speed(PWM_MAX);}
	else if(PWM_D<-PWM_MAX){Motor4_Speed(-PWM_MAX);}
	else{Motor4_Speed((int16_t)PWM_D);}
}
void agv_moveForward(void)
{
	PWMLevelSet(PWM15_0,ENABLE,DISABLE);	
	PWMLevelSet(PWM15_3,ENABLE,DISABLE);
	PWMLevelSet(PWM15_4,ENABLE,DISABLE);	
	PWMLevelSet(PWM15_7,ENABLE,DISABLE);

	PWMLevelSet(PWM15_1,DISABLE,DISABLE);
	PWMLevelSet(PWM15_2,DISABLE,DISABLE);
	PWMLevelSet(PWM15_5,DISABLE,DISABLE);
	PWMLevelSet(PWM15_6,DISABLE,DISABLE);
}
void agv_moveBack(void)
{
	PWMLevelSet(PWM15_1,ENABLE,DISABLE);
	PWMLevelSet(PWM15_2,ENABLE,DISABLE);
	PWMLevelSet(PWM15_5,ENABLE,DISABLE);
	PWMLevelSet(PWM15_6,ENABLE,DISABLE);

	PWMLevelSet(PWM15_0,DISABLE,DISABLE);	
	PWMLevelSet(PWM15_3,DISABLE,DISABLE);
	PWMLevelSet(PWM15_4,DISABLE,DISABLE);	
	PWMLevelSet(PWM15_7,DISABLE,DISABLE);
}
void agv_stop(void)
{
	PWMLevelSet(PWM15_0,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_1,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_2,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_3,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_4,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_5,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_6,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
	PWMLevelSet(PWM15_7,ENABLE,DISABLE);	//PWM_ID, 强制输出低电平, 强制输出高电平
}
float binaryToFloat(unsigned char b0, unsigned char b1, unsigned char b2, unsigned char b3) {
    float f;
    unsigned char *pf = (unsigned char *)&f;
    pf[0] = b3;
    pf[1] = b2;
    pf[2] = b1;
    pf[3] = b0;
    return f;
}
void fuc_agv_setRun(u8* buf)
{
	agv_target_Vy.f=binaryToFloat(buf[1],buf[2],buf[3],buf[4]);

	if(agv_target_Vx.f>agv_limit_rateSpeed.f)agv_target_Vx.f=agv_limit_rateSpeed.f; //限制角速度
	else if(agv_target_Vx.f<-agv_limit_rateSpeed.f)agv_target_Vx.f=-agv_limit_rateSpeed.f; //限制角速度
	if(agv_target_Vy.f>agv_limit_lineSpeed.f)agv_target_Vy.f=agv_limit_lineSpeed.f; //限制角速度
	else if(agv_target_Vy.f<-agv_limit_lineSpeed.f)agv_target_Vy.f=-agv_limit_lineSpeed.f; //限制角速度

	printf("目标线速度为%0.3fm/s;",agv_target_Vy.f);
	compare=agv_target_Vy.f*2048;//满速为2m/s,满占空比为0x1000=4096
	Motor1_Speed(compare);
	Motor2_Speed(compare);
	Motor3_Speed(compare);
	Motor4_Speed(compare);
//	printf("目标角速度为%.3frad/s;",agv_target_Vx.f);
}
int16_t pwm_LF,pwm_RF,pwm_LB,pwm_RB;
void fuc_pwm_control(u8* buf)
{
	
	pwm_LF=buf[1]<<8|buf[0];
	pwm_RF=buf[3]<<8|buf[2];
	pwm_LB=buf[5]<<8|buf[4];
	pwm_RB=buf[7]<<8|buf[6];
	Motor1_Speed(pwm_LF);
	Motor2_Speed(pwm_RF);
	Motor3_Speed(pwm_LB);
	Motor4_Speed(pwm_RB);
	printf("hlly333A轮速度为%d;",pwm_LF);				
}
/////////////////////////////////     无线手柄控制     ///////////////////////////////////////////////////////////
void handle_process(void)
{
	if(hd.flag_onOff)//遥控器使能
	{
		switch(hd.mode)//遥控模式[0x00,0x01,0x02,0x03,0x04],[默认,左摇杆模式,右摇杆模式,持续运动模式，点动模式]
		{	
			case 0:
				break;
			case 1:
				if(abs(hd.AL1-0x7f)>10)//摇杆水平方向控制前进左右速度
				{
					if(hd.AL1>0x7f)agv_target_Vy.f=(float)(hd.AL1-0x7f-10)/118*2;
					else agv_target_Vy.f=(float)(hd.AL1-0x7f+10)/118*0.7f;
				}
				else agv_target_Vy.f=0;
				if(abs(hd.AL2-0x7f)>10)//摇杆竖直方向控制控制前后速度
				{
					if(hd.AL2>0x7f)agv_target_Vx.f=-(float)(hd.AL2-0x7f-10)/118*2;//摇杆中值为0x7f;最小有效速度为10/180*2；有效量程为118，即10-256；满速为2m/s
					else agv_target_Vx.f=-(float)(hd.AL2-0x7f+10)/118*2;
				}
				else agv_target_Vx.f=0;
				printf("左右速度为%f;",agv_target_Vy.f);				
				printf("前后速度为%f;",agv_target_Vx.f);
				Move_Transfrom(agv_target_Vx.f,agv_target_Vy.f,0);
//				fuc_agv_run();
				break;
			case 2:
				break;
			case 3:
//				if(hd.LD)agv_target_Vy.f=0,agv_target_Vx.f=0;
//				else if(hd.LU&hd.LL)agv_target_Vy.f=flash_p.agv_default_lineSpeed.f;//上、左同时按
//				else if(hd.LU&hd.LR)agv_target_Vy.f=flash_p.agv_default_lineSpeed.f;//上、右同时按
//				else if(hd.LU)agv_target_Vy.f=flash_p.agv_default_lineSpeed.f,agv_target_Vx.f=0;
//				else if(hd.LL)agv_target_Vx.f=flash_p.agv_default_rateSpeed.f,agv_target_Vy.f=0;
//				else if(hd.LR)agv_target_Vx.f=-flash_p.agv_default_rateSpeed.f,agv_target_Vy.f=0;
//				fuc_agv_run();
				break;
			case 4:
//				if(hd.RU)agv_targetDistance.f=0.01,agv_currentDistance.f=0,fuc_agv_toDistance();
//				else if(hd.RD)agv_targetDistance.f=-0.01,agv_currentDistance.f=0,fuc_agv_toDistance();
//				else if(hd.RL)agv_targetAngle.f=agv_currentAngle.f+3,fuc_normAngle(&agv_targetAngle.f),fuc_agv_toAngle();
//				else if(hd.RR)agv_targetAngle.f=agv_currentAngle.f-3,fuc_normAngle(&agv_targetAngle.f),fuc_agv_toAngle();
				
				break;
		}
	}
}